%% 多飞行器再入段时间协同弹道规划方法 %%
% 准平衡滑翔条件 + 给定攻角剖面 -> 倾侧角走廊
clc;
close all;
clear;
g = 9.8;
deg2rad = pi/180;
KQ = 9.4369e-5;
dQmax = 4e6;
qmax = 70e3;
nmax = 4*g;
mass = 907.2;
S = 0.484;
V = (2e3:100:8e3)'; % V 2000~8000m/s
h = 65e3;
R0 = 6378145;
r = h + R0; % r 暂时设为定值
v1 = 4700;
v2 = 3100;
alphaMax = 20*deg2rad;
alphaLD = 12*deg2rad;

alpha = zeros(length(V),1);
for i = 1:length(V)
    if V(i) > v1
        alpha(i,1) = alphaMax;
    elseif V(i) > v2
        alpha(i,1) = (alphaLD - alphaMax)*(V(i) - v1)/(v2 - v1) + alphaMax;
    else
        alpha(i,1) = alphaLD;
    end
end
[~,sos,~,rho] = atmoscoesa(r - R0);
Ma = V./sos;
alphadeg = alpha*180/pi;
[CL,CD] = getCLCD(alpha, Ma);

% 式18
bankQmax = acos((g-V.^2/r)*2*mass.*V.^(4.3)*(KQ/dQmax)^2./(S*CL))./deg2rad;
bankqmax = acos((g-V.^2/r).*mass./(qmax.*CL*S))./deg2rad;
banknmax = acos((g-V.^2/r).*sqrt((CD./CL).^2+1)./nmax)./deg2rad;
f = figure('Name', '倾侧角走廊');
plot(V,bankQmax,V,bankqmax,V,banknmax);hold;
plot(V,-bankQmax,V,-bankqmax,V,-banknmax);